#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2

class CameraPublisher(Node):
    def __init__(self):
        super().__init__('camera_publisher')
        self.publisher_ = self.create_publisher(Image, 'camera/image_raw', 10)
        self.timer_ = self.create_timer(0.1, self.timer_callback)  # 每100ms发布一次
        self.bridge = CvBridge()
        self.cap = cv2.VideoCapture(0)  # 打开默认摄像头
        if not self.cap.isOpened():
            self.get_logger().error("Unable to open camera")

    def timer_callback(self):
        ret, frame = self.cap.read()
        if ret:
            # 将OpenCV图像转换为ROS 2图像消息
            msg = self.bridge.cv2_to_imgmsg(frame, encoding='bgr8')
            self.publisher_.publish(msg)
            self.get_logger().info('Publishing image')
        else:
            self.get_logger().error('Failed to capture frame')

    def destroy_node(self):
        # 释放摄像头资源
        self.cap.release()
        super().destroy_node()

def main(args=None):
    rclpy.init(args=args)
    camera_publisher = CameraPublisher()
    try:
        rclpy.spin(camera_publisher)
    except KeyboardInterrupt:
        camera_publisher.get_logger().info('Shutting down')
    finally:
        camera_publisher.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()